/*******************************************************************************
 * Copyright (C) 2013 Jeremy Brodt and Kevin Gillespie
 * Description: Wheel Encoder Driver
 *         $Id: encoder.c 49 2014-05-01 21:06:14Z brodtj1212@gmail.com $
 *******************************************************************************
 */

#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "config.h"
#include "errors.h"
#include "tmr.h"

#include "car.h"
#include "encoder.h"

// RTOS includes
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"

/***** Definitions *****/
#define TIMER_FREQ            (SystemPeriphClock / 64)

#define MAX_TICKS_TO_SUM      16
#define MAX_TICKS_SHIFT       4

#define NUM_TICKS_CUTOFF      5000

#ifndef CAPTURE_BUFFER_SIZE
#define CAPTURE_BUFFER_SIZE   32
#endif

typedef struct {
  int wheel;
  uint32_t value;
} enc_capture_t;


/***** Global Data *****/
volatile uint32_t last_capture[NUM_WHEELS];
volatile unsigned int overflow_cnt[NUM_WHEELS];
volatile unsigned int valid_cnt[NUM_WHEELS];
volatile int32_t odometer[NUM_WHEELS];
volatile unsigned int delta_index[NUM_WHEELS];
volatile uint32_t delta_buffer[NUM_WHEELS][CAPTURE_BUFFER_SIZE];
volatile unsigned int direction[NUM_WHEELS];

#define DELTA_BUFFER_SIZE   100
volatile unsigned int first_delta_index;
volatile uint32_t first_delta_buffer[DELTA_BUFFER_SIZE][NUM_WHEELS];
volatile unsigned int recent_delta_index;
volatile uint32_t recent_delta_buffer[DELTA_BUFFER_SIZE][NUM_WHEELS];

int32_t alarm_count[NUM_WHEELS];
void (*alarm_callback[NUM_WHEELS])(void);

const uint16_t hundred_mm_per_cnt[NUM_WHEELS] = {
  (((WHEEL_CIRCUM_10MM_LEFT * 10) + (TICKS_PER_REV / 2)) / TICKS_PER_REV),
  (((WHEEL_CIRCUM_10MM_RIGHT * 10) + (TICKS_PER_REV / 2)) / TICKS_PER_REV),
};

const uint16_t wheel_circum_10mm[NUM_WHEELS] = {
  WHEEL_CIRCUM_10MM_LEFT,
  WHEEL_CIRCUM_10MM_RIGHT
};

const uint16_t mms7_per_cnt[NUM_WHEELS] = {
  (((WHEEL_CIRCUM_MMS4_LEFT << 3) + (TICKS_PER_REV >> 1)) / TICKS_PER_REV),
  (((WHEEL_CIRCUM_MMS4_RIGHT << 3) + (TICKS_PER_REV >> 1)) / TICKS_PER_REV),
};

/***** File Scope Data *****/
static const mxc_tmr_regs_t *tmr_base[2] = { TMR_ENC_L, TMR_ENC_R };
static xQueueHandle xQueue;

/***** Function Prototypes *****/
static void capture_task(void * pvParameters);

/****************************************************************************/
int enc_init(void)
{
  int err;
  tmr_cfg_t config;
  unsigned int wheel;

  // Configure timers for capture
  config.mode = TMR_CAPTURE;
  config.cmp_cnt = 0xFFFFFFFF;  // set to max value
  config.pol = 0;

  for (wheel = 0; wheel < NUM_WHEELS; wheel++) {
    // Clear the state
    overflow_cnt[wheel] = 0xFFFFFFFF;
    odometer[wheel] = 0;
    delta_index[wheel] = 0;
    last_capture[wheel] = 0;
    valid_cnt[wheel] = 0;
    alarm_count[wheel] = 0;
    alarm_callback[wheel] = NULL;
    memset((void*)delta_buffer[wheel], 0xFF, sizeof(delta_buffer[wheel]));
    direction[wheel] = FORWARD;

    // Configure and enable the timer
    if ((err = tmr_init((mxc_tmr_regs_t*)tmr_base[wheel], MXC_S_TMR_CN_PRES_PDIV_BY_64)) != E_NO_ERROR) {
      return err;
    }

    if ((err = tmr_cfg((mxc_tmr_regs_t*)tmr_base[wheel], &config)) != E_NO_ERROR) {
      return err;
    }
    tmr_enable(TMR_ENC_L);
  }

  NVIC_EnableIRQ(TMR0_IRQn);
  NVIC_EnableIRQ(TMR2_IRQn);

  enc_log_clear();

  // Create a queue for passing capture values from the ISR
  xQueue = xQueueCreate(8, sizeof(enc_capture_t));
  if (xQueue == NULL) {
    printf("ERROR: Failed to create encoder queue\n");
    return E_NONE_AVAIL;
  }

  // Create the capture task
// TODO - set priority
  if (xTaskCreate(capture_task, (const char * const)"capture", 200, NULL, 5, NULL) != pdPASS) {
    return E_NONE_AVAIL;
  }

  return E_NO_ERROR;
}

/****************************************************************************/
static uint32_t getdelta(unsigned int wheel, uint32_t capture)
{
  uint32_t delta;

  if (capture > last_capture[wheel])
    delta = capture - last_capture[wheel];
  else 
    delta = 0xFFFFFFFF - last_capture[wheel] + capture + 1;

  return delta;
}

/****************************************************************************/
unsigned int enc_get_avg(unsigned int wheel)
{
  int sample, index;
  uint32_t sum = 0;
  unsigned int howmany;
  unsigned int avg = 0xFFFFFFFF;
  uint32_t most_recent_sample, oldest_sample, current_delta;

  if (valid_cnt[wheel] > 1) {

    // Accumulate as many samples as we can up to max
    howmany = MAX_TICKS_TO_SUM;
    if (howmany > valid_cnt[wheel])
      howmany = valid_cnt[wheel];
  
    // Initialize index and most recent sample
    index = delta_index[wheel] - 1;
    if (index < 0) { index = CAPTURE_BUFFER_SIZE - 1; }
    most_recent_sample = delta_buffer[wheel][index];
  
    // Accumulate samples
    oldest_sample = 0;
    for (sample = 0; sample < howmany; sample++) {
      oldest_sample = delta_buffer[wheel][index];
      sum += oldest_sample;
      if (--index < 0) { index = CAPTURE_BUFFER_SIZE - 1; }
    }

    // Calculate average
    if (howmany == MAX_TICKS_TO_SUM)
      avg = sum >> MAX_TICKS_SHIFT;
    else 
      avg = sum / howmany;
  
    // Eliminate glitches in most recent sample
    if ( (avg > most_recent_sample) && (valid_cnt[wheel] > howmany) && ((avg - most_recent_sample) > (avg >> 3)) ) {
      sum -= most_recent_sample;
      sum += delta_buffer[wheel][index];
    }
  
    // Improve response time when slowing down
    // If we are slowing down, the current delta count value may have additional information
    current_delta = getdelta(wheel, tmr_base[wheel]->cnt);
    if (current_delta > avg) {
      sum -= oldest_sample;
      sum += current_delta;
    }

    if (howmany == MAX_TICKS_TO_SUM)
      avg = sum >> MAX_TICKS_SHIFT;
    else
      avg = sum / howmany;
  }

  return avg;
}

/****************************************************************************/
unsigned int enc_get_speed(unsigned int wheel)
{
  uint32_t avg;
  unsigned int speed = 0;

  avg = enc_get_avg(wheel);
  if (avg < 0xFFFFFFFF)
    speed = (((uint32_t)TIMER_FREQ * wheel_circum_10mm[wheel]) / (avg * TICKS_PER_REV * 10));

  return speed;
}

/****************************************************************************/
void enc_alarm_set(unsigned int wheel, int32_t count, void (*func)(void))
{
  alarm_count[wheel] = odometer[wheel] + count;
  alarm_callback[wheel] = func;

  if (odometer[wheel] == alarm_count[wheel]) {
    if (alarm_callback[wheel] != NULL)
      alarm_callback[wheel]();
    alarm_callback[wheel] = NULL;
    alarm_count[wheel] = NO_ALARM;
  }
}

/****************************************************************************/
void enc_alarm_clr(unsigned int wheel)
{
  alarm_count[wheel] = NO_ALARM;
  alarm_callback[wheel] = NULL;
}

/****************************************************************************/
void enc_direction_change(unsigned int wheel, unsigned int new_direction)
{
  if (direction[wheel] == new_direction)
    return;

  if (valid_cnt[wheel] != 0)
    printf("ERROR: direction change when valid_cnt != 0\n");

  direction[wheel] = new_direction;
  valid_cnt[wheel] = 0;
}

/****************************************************************************/
static void capture_task(void * pvParameters)
{
  enc_capture_t capture;
  int index, wheel;
  uint16_t delta;
  void (*temp_callback)(void);

  if (xQueue == NULL)
    vTaskSuspend(NULL);

  while(1) {
    // Wait indefinitely for a capture in the queue
    if (xQueueReceive(xQueue, &capture, portMAX_DELAY) != pdTRUE) {
      printf("ERROR: xQueueReceive error in Encoder\n");
    }

    wheel = capture.wheel;

    // If we already have a capture value
    if (valid_cnt[wheel] > 0) {

      delta = getdelta(wheel, capture.value);
      index = delta_index[wheel];

      delta_buffer[wheel][index] = delta;
      if (++delta_index[wheel] >= CAPTURE_BUFFER_SIZE) { delta_index[wheel] = 0; }

      if (first_delta_index < DELTA_BUFFER_SIZE) {
        first_delta_buffer[first_delta_index][wheel] = delta;
        first_delta_index++;
      }
    }

    // Recode last capture
    last_capture[wheel] = capture.value;

    // Update the valid capture count
    if (valid_cnt[wheel] < 0xFFFFFFFF)
      valid_cnt[wheel]++;

    // Update the odometer depending on our direction of travel
    if (direction[wheel] == FORWARD)
      odometer[wheel]++;
    else
      odometer[wheel]--;

    // Reset the overflow count
    overflow_cnt[wheel] = 0;

    // Check the alarm
    if (odometer[wheel] == alarm_count[wheel]) {
      temp_callback = alarm_callback[wheel];
      alarm_callback[wheel] = NULL;
      alarm_count[wheel] = NO_ALARM;
      if (temp_callback != NULL)
        temp_callback();
    }
  }
}

/****************************************************************************/
void enc_log_display(void)
{
  int i, l, r;

  l = delta_index[LEFT];
  r = delta_index[RIGHT];

  printf("\n***** Encoder Log *****\n");
  for (i = 0; i < CAPTURE_BUFFER_SIZE; i++, l++, r++) {
    if (l == CAPTURE_BUFFER_SIZE) { l = 0; }
    if (r == CAPTURE_BUFFER_SIZE) { r = 0; }
    printf("%3d %4x %4x\n", i, (unsigned int)delta_buffer[LEFT][l], (unsigned int)delta_buffer[RIGHT][r]);
  }
}

/****************************************************************************/
void enc_log_clear(void)
{
  memset((void*)first_delta_buffer, 0, sizeof(first_delta_buffer));
  first_delta_index = 0;
  memset((void*)recent_delta_buffer, 0, sizeof(recent_delta_buffer));
  recent_delta_index = 0;
}

/****************************************************************************/
void enc_log_display_first(void)
{
  int i;

  printf("\n***** First Encoder Log *****\n");
  for (i = 0; i < DELTA_BUFFER_SIZE; i++) {
    printf("%3d %4x %4x\n", i, (unsigned int)first_delta_buffer[i][LEFT], (unsigned int)first_delta_buffer[i][RIGHT]);
  }
}

/****************************************************************************/
void enc_log_display_recent(void)
{
  int i;

  printf("\n***** Recent Encoder Log *****\n");
  for (i = recent_delta_index; i < DELTA_BUFFER_SIZE; i++) {
    printf("%3d %4x %4x\n", i - recent_delta_index, (unsigned int)recent_delta_buffer[i][LEFT], (unsigned int)recent_delta_buffer[i][RIGHT]);
  }
  for (i = 0; i < recent_delta_index; i++) {
    printf("%3d %4x %4x\n", i + (DELTA_BUFFER_SIZE - recent_delta_index), (unsigned int)recent_delta_buffer[i][LEFT], (unsigned int)recent_delta_buffer[i][RIGHT]);
  }
}

#if 0
// TODO - FIXME - need to set up a timeout to determine when wheel has stopped
//******************************************************************************
void timer_isr(void)
{
  // TB0 Overflow
  if (TB0CN & 0x80) {
    TB0CN &= ~0x80;
    if (overflow_cnt[0] < 3)
      overflow_cnt[0]++;
    if (overflow_cnt[0] == 2) {
      valid_cnt[0] = 0;
      delta_buffer[0][delta_index[0]] = 0xFFFFFFFF;
      if (++delta_index[0] >= CAPTURE_BUFFER_SIZE) { delta_index[0] = 0; }
    }
  }

  // TB1 Overflow
  if (TB1CN & 0x80) {
    TB1CN &= ~0x80;
    if (overflow_cnt[1] < 3)
      overflow_cnt[1]++;
    if (overflow_cnt[1] == 2) {
      valid_cnt[1] = 0;
      delta_buffer[1][delta_index[1]] = 0xFFFFFFFF;
      if (++delta_index[1] >= CAPTURE_BUFFER_SIZE) { delta_index[1] = 0; }
    }
  }
}
#endif

//******************************************************************************
void TMR0_IRQHandler(void)
{
  enc_capture_t capture;
  BaseType_t xHigherPriorityTaskWoken;

  // Clear the interrupt flag
  MXC_TMR0->intr = 0;

  // Get the capture value
  capture.wheel = LEFT;
  capture.value = MXC_TMR0->pwm;

  // Send it to the processing task
  if (xQueueSendFromISR(xQueue, &capture, &xHigherPriorityTaskWoken) != pdTRUE) {
    printf("ERROR: xQueueSend(xQueueEncL) failed\n");
  }

  // Switch tasks if necessary
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

//******************************************************************************
void TMR2_IRQHandler(void)
{
  enc_capture_t capture;
  BaseType_t xHigherPriorityTaskWoken;

  // Clear the interrupt flag
  MXC_TMR2->intr = 0;

  // Get the capture value
  capture.wheel = RIGHT;
  capture.value = MXC_TMR2->pwm;

  // Send it to the processing task
  if (xQueueSendFromISR(xQueue, &capture, &xHigherPriorityTaskWoken) != pdTRUE) {
    printf("ERROR: xQueueSend(xQueueEncR) failed\n");
  }

  // Switch tasks if necessary
  portYIELD_FROM_ISR(xHigherPriorityTaskWoken);
}

